# 功能
import threading
import struct
import cv2 as cv
import numpy as np
# import rospy
# from nav_msgs.msg import OccupancyGrid
import zlib
# 通信
import socket

paint = 0


# -----------网络通信-----------------
class TelecomVideo:
    @staticmethod
    def split_img(img):  # 分割图片数据
        gen = []
        img_encode = cv.imencode('.jpg', img, [int(cv.IMWRITE_JPEG_QUALITY), 50])[1]  # 将图片转化为jpg格式数据流
        data = np.array(img_encode)  # array格式
        for i in range(0, len(data), 2048):  # 每块长度2048的数据截出
            gen.append(data[i:i + 2048])
        return gen, len(data)

    @staticmethod
    def th_send(client, address, data, port):
        client.connect((address, port))
        client.send(data)
        client.close()

    def send_video(self, address):
        global paint
        udp_info = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  # 图片信息传输
        udp_info.connect((address, 1000))
        video = cv.VideoCapture(0)
        video.set(cv.CAP_PROP_FPS, 24)
        while video.isOpened():
            ret, frame = video.read()
            chunks, length = self.split_img(frame)  # 将图片分割为多个包，总长度
            info = struct.pack('ii', length, paint)
            udp_info.send(info)  # 发送长度信息
            for index, chunk in enumerate(chunks):  # 根据包的个数动态创建多线程发送包
                chunk = zlib.compress(chunk.tobytes(), 6)  # 将包转换bytes
                packs = struct.pack('ii{}s'.format(len(chunk)), paint, index, chunk)  # 打包帧，序号，图片
                print(len(packs))
                udp_video = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)  # 发送图片数据
                th = threading.Thread(target=self.th_send, args=[udp_video, address, packs, 1001 + index], daemon=True)
                th.start()
                index += 1
            index = 0
            paint += 1
        video.release()


class TelecomMap:
    def __init__(self, address):  # 初始化地图数据
        self.map_data = []
        self.address = address

    def callback(self, data):  # 回调更新地图数据
        self.map_data = data.data
        self.communicate()

    def communicate(self):  # 地图数据传输
        for index, i in enumerate(range(0, len(self.map_data), 4096)):
            # udp_map连接
            udp_map = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
            udp_map.connect((self.address, 9988))
            # chunk → tobytes → compress，打包成struct发送
            chunk = np.array(self.map_data[i:i + 4096], dtype=np.int8).tobytes()
            chunk_zip = zlib.compress(chunk, level=6)
            pack = struct.pack("I{}s".format(len(chunk_zip)), index, chunk_zip)
            udp_map.send(pack)

    def map_send(self):
        # In ROS, nodes are uniquely named. If two nodes with the same
        # name are launched, the previous one is kicked off. The
        # anonymous=True flag means that rospy will choose a unique
        # name for our 'listener' node so that multiple listeners can
        # run simultaneously.
        rospy.init_node('map_read', anonymous=True)
        rospy.Subscriber("map", OccupancyGrid, self.callback)
        # spin() simply keeps python from exiting until this node is stopped
        rospy.spin()


class TelecomAction:
    def __init__(self, address):
        self.address = address

    def send_action(self, message: bytes):
        udp_action = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        udp_action.connect((self.address, 6666))
        ziped = zlib.compress(message)
        udp_action.send(ziped)
